#include <iostream>
#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>
#include <string>
#include <unistd.h>

using namespace std;

struct CameraIntr{
    double cx;
    double cy;
    double fx;
    double fy;
    double depthScale;
}cameraIntr;

//    0.00103038
//   0.00184441
// -0.000725832
//     0.999998

//  -5.2934964534264488e-02, 3.8470642197557526e-04,
//        1.9941897903097364e-03 

// rosrun tf static_transform_publisher -0.0529 0.0004 0.002 0.00103 0.00184 -0.000726  0.999998 platform camera 100 

string frame_id;
string topic_id;
string rgbPath;
string depthPath;
string node_name;
float rate;

int getParam(ros::NodeHandle nh)
{
    node_name = ros::this_node::getName();

    ROS_INFO("node_name: %s", node_name.c_str());

    nh.param<std::string>(node_name+"/topic_id", topic_id,"points");
    nh.param<std::string>(node_name+"/frame_id", frame_id,"platform");
    nh.param<std::string>(node_name+"/rgbPath", rgbPath,"/home/ubuntu/code/rgbd2pcd/data/rgb.png");
    nh.param<std::string>(node_name+"/depthPath", depthPath,"/home/ubuntu/code/rgbd2pcd/data/depth.png");
    nh.param<double>(node_name+"/cameraIntr/cx",cameraIntr.cx,319.5);
    nh.param<double>(node_name+"/cameraIntr/cy",cameraIntr.cy,239.5);
    nh.param<double>(node_name+"/cameraIntr/fx",cameraIntr.fx,525.0);
    nh.param<double>(node_name+"/cameraIntr/fy",cameraIntr.fy,525.0);
    nh.param<double>(node_name+"/cameraIntr/depthScale",cameraIntr.depthScale,5000.0);
    nh.param<float>(node_name+"/rate",rate,1.0);
    return 0;
}

pcl::PointCloud<pcl::PointXYZRGB> rgbd2pcd(string rgbPath, string depthPath, CameraIntr &ci){
    // 数据读取
    cv::Mat rgb, depth;
    rgb = cv::imread(rgbPath);
    depth = cv::imread(depthPath, -1);
    
    // Intrinsics parameters
    // double cx = 319.5;
    // double cy = 239.5;
    // double fx = 525.0;
    // double fy = 525.0;
    // double depthScale = 5000.0;
    double cx = ci.cx;
    double cy = ci.cy;
    double fx = ci.fx;
    double fy = ci.fy;
    double depthScale = ci.depthScale;

    // define point type of pointcloud：XYZRGB.
    typedef pcl::PointXYZRGB PointT; 
    typedef pcl::PointCloud<PointT> PointCloud;
    
    // calculate XYZRGB value of every point.
    PointCloud pointCloud;

    for ( int v=0; v<rgb.rows; v++ )
        for (int u=0; u<rgb.cols; u++)
        {
            unsigned int d = depth.ptr<unsigned short>(v)[u];
            if (d==0)
                continue;
            PointT p;
            p.z = double(d)/depthScale;
            p.x = (u-cx)*p.z/fx;
            p.y = (v-cy)*p.z/fy;
            p.b = rgb.data[v*rgb.step+u*rgb.channels()];
            p.g = rgb.data[v*rgb.step+u*rgb.channels()+1];
            p.r = rgb.data[v*rgb.step+u*rgb.channels()+2];
            pointCloud.points.push_back(p);
        }
        
    pointCloud.is_dense = false;

    ROS_INFO("Number of points: %d",(int)pointCloud.size());
    return pointCloud;
}

int main(int argc,char** argv)
{
    ros::init (argc, argv, "empty");

    ros::NodeHandle nh;

    ROS_INFO("Current working directory: %s",get_current_dir_name());

    getParam(nh);

    ros::Publisher pcl_pub = nh.advertise<sensor_msgs::PointCloud2> (topic_id, 1);

    pcl::PointCloud<pcl::PointXYZRGB> rviz_cloud = rgbd2pcd(rgbPath,depthPath,cameraIntr);

    sensor_msgs::PointCloud2 rviz_output;

    pcl::toROSMsg(rviz_cloud, rviz_output);

    rviz_output.header.frame_id = frame_id;

    ros::Rate loop_rate(rate);

    while (ros::ok())
    {
        pcl_pub.publish(rviz_output);
        // ros::spinOnce();
        loop_rate.sleep();
    }

    return 0;
}
